/*
 * Author  : Leo <lipf160627@163.com>
 * Created : 2025-8-27
 * Desc    : Car posture controller
 */

#include "posture.h"

#include <string.h>

// 前进/后退/左转/右转/停
typedef enum
{
    POSTURE_STOP = 0,
    POSTURE_FORWARD,
    POSTURE_BACKWARD,
    POSTURE_LEFT,
    POSTURE_RIGHT,
} Posture_Cmd_t;

typedef struct
{
    Posture_Cmd_t cmd;
    union
    {
        struct
        {
            uint16_t distanceTarget;
            uint16_t distanceCurrent;
            uint8_t  speed;
        } StraightInfo;
        struct
        {
            uint8_t angleTarget;
            uint8_t angleCurrent;
            uint8_t speed;
        } turnInfo;
    };
} Posture_Action_t;

typedef struct
{
    float kp;
    float kd;
    float ki;
    float error;
    float error_last;
    float error_sum;
    float output;
} PID_t;

// static PID_t pid_roll;

Posture_Action_t postureAction;

static void Posture_ActionReset(void)
{
    // POSTURE_STOP = 0, initial state
    memset(&postureAction, 0, sizeof(postureAction));
}

static void Posture_Straight(void)
{
    if (0 == postureAction.StraightInfo.distanceTarget) {
        postureAction.StraightInfo.distanceCurrent += postureAction.StraightInfo.speed;
    }
    else if (postureAction.StraightInfo.distanceCurrent < postureAction.StraightInfo.distanceTarget) {
        postureAction.StraightInfo.distanceCurrent += postureAction.StraightInfo.speed;
    }
    else {
        Posture_ActionReset();
    }
}

static void Posture_Turn(void)
{
    if (0 == postureAction.turnInfo.angleTarget) {
        postureAction.turnInfo.angleCurrent += postureAction.turnInfo.speed;
    }
    else if (postureAction.turnInfo.angleCurrent < postureAction.turnInfo.angleTarget) {
        postureAction.turnInfo.angleCurrent += postureAction.turnInfo.speed;
    }
    else {
        Posture_ActionReset();
    }
}

bool Posture_Forward(uint16_t distance, uint8_t speed)
{
    Posture_ActionReset();
    postureAction.cmd = POSTURE_FORWARD;
    postureAction.StraightInfo.distanceTarget = distance;
    postureAction.StraightInfo.speed          = speed;

    return true;
}

bool Posture_Backward(uint16_t distance, uint8_t speed)
{
    Posture_ActionReset();
    postureAction.cmd = POSTURE_BACKWARD;
    postureAction.StraightInfo.distanceTarget = distance;
    postureAction.StraightInfo.speed          = speed;

    return true;
}

bool Posture_TurnLeft(uint8_t angle, uint8_t speed)
{
    Posture_ActionReset();
    postureAction.cmd = POSTURE_LEFT;
    postureAction.turnInfo.angleTarget = angle;
    postureAction.turnInfo.speed       = speed;

    return true;
}

bool Posture_TurnRight(uint8_t angle, uint8_t speed)
{
    Posture_ActionReset();
    postureAction.cmd = POSTURE_RIGHT;
    postureAction.turnInfo.angleTarget = angle;
    postureAction.turnInfo.speed       = speed;

    return true;
}

bool Posture_Stop(void)
{
    Posture_ActionReset();

    return true;
}

bool Posture_Terminate(void)
{
    Posture_ActionReset();

    return true;
}

void Posture_Run(void)
{
    switch (postureAction.cmd) {
        case POSTURE_FORWARD:
        case POSTURE_BACKWARD:
            Posture_Straight();
            break;
        case POSTURE_LEFT:
        case POSTURE_RIGHT:
            Posture_Turn();
            break;
        case POSTURE_STOP:
            Posture_ActionReset();
            break;
        default:
            break;
    }
}